#include "ctrackerwindow.h"
#include "ctracker.h"
#include "crope.h"
#include "ccloth.h"

#include <buola/gui/cmenu.h>
#include <buola/widgets/ctoolbar.h>
#include <buola/scene/cscene.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/controllers/ccameracontroller.h>

namespace buola { namespace tracker {

static const double sScale=100.0;
static const mat::CVec3d sCameraAt{0,1.0*sScale,-0.98*sScale};
static const mat::CVec3d sCameraPos{0,0,0};
static const mat::CVec3d sCameraUp{0,0,1};

cvi::CCloud_rgb8 load_cloud(const io::CURI &pURI,int pFrame)
{
   cvi::CCloud_rgb8 lCloud=cvi::load_cloud_pcd(pURI+(to_string(pFrame)+".pcd"));
   lCloud=cvi::scale_cloud(lCloud,sScale);
   lCloud=cvi::transform_lookat(lCloud,mat::CVec3f(sCameraAt),mat::CVec3f(sCameraPos),mat::CVec3f(sCameraUp));
   return lCloud; 
}

// cvi::CCloud_rgb8 load_cloud(const io::CURI &pURI,int pFrame)
// {
//     cvi::CCloud_rgb8 lCloud;
//     io::CTextReader lCloudStream(io::file("/home/xavi/test/bulletsim/cloudpoints"));
// 
//     for(int i=0;i<195;i++)
//     {
//         typename cvi::CCloud_rgb8::TPoint lPoint;
//         lPoint.mPixel={0,0,0};
//         lCloudStream >> lPoint.mPoint.x() >> lPoint.mPoint.y() >> lPoint.mPoint.z();
//         lCloud.push_back(lPoint);
//     }
//     
//     return lCloud; 
// }
// 
CTrackerWindow::CTrackerWindow(const io::CURI& pURI)
    :   mURI(pURI)
    ,   mFrame(0)
{
    mTimer.sTimer.Connect([&]{DoStep();});

    SetupWindow();
    SetupInput();
    SetupScene();
    SetupViews();

    AddCommitHook([this](const CRegion &pRegion){OnCommitHook(pRegion);});
}

CTrackerWindow::~CTrackerWindow()
{
}

void CTrackerWindow::SetupWindow()
{
    SetNumRows(3);
    mToolBar=new gui::CToolBar;
    Attach(mToolBar,gui::ATTACH_NORTH,gui::ATTACH_TOOLBAR);

    gui::PMenu lMenu=new gui::CMenu;
    
    lMenu->Add(gui::new_menu_item(L"next iteration",ICON_STOCK_ARROW_RIGHT))->
              eTriggered.Connect(&CTrackerWindow::DoPlay,this);
    lMenu->Add(gui::new_menu_item(L"25 iterations",ICON_STOCK_ARROW_RIGHT_DOUBLE))->
              eTriggered.Connect(&CTrackerWindow::DoStep,this);
    
    mToolBar->SetMenu(lMenu);
}

void CTrackerWindow::SetupInput()
{
    mCloud=load_cloud(mURI,0);
    mObject.reset(new CRope(mCloud));
}

void CTrackerWindow::SetupScene()
{
    mCloudScene=new scene::CScene(true,false);
    mObjectScene=new scene::CScene(true,false);
    
    mObject->AddToScene(mObjectScene);
    
    mSceneCloud=new scene::CPointCloud();
    mSceneCloud->SetCloud(clone(mCloud));
    mCloudScene->World()->AddObject(mSceneCloud);
}

void CTrackerWindow::SetupViews()
{
    {
        mCloudSceneView=new scene::CSceneView;
        mCloudSceneView->AddScene(mCloudScene);

        scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
        lCamera->SetFOV(2*atan2(240.0,525.0));
        lCamera->SetClipping(0.1*sScale,50.0*sScale);
        lCamera->LookAt(sCameraAt,sCameraPos,sCameraUp);
        mCloudSceneView->SetCamera(lCamera);

        mCloudSceneView->AddController(new scene::CCameraController(lCamera));
        mCloudSceneView->Resize({800,600});
        mCloudSceneView->SetTitle(L"original cloud");
        Add(mCloudSceneView);
    }
    
    {
        mObjectSceneView=new scene::CSceneView;
        mObjectSceneView->AddScene(mObjectScene);

        scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
        lCamera->SetFOV(2*atan2(240.0,525.0));
        lCamera->SetClipping(0.1*sScale,50.0*sScale);
        lCamera->LookAt(sCameraAt,sCameraPos,sCameraUp);
        mObjectSceneView->SetCamera(lCamera);

        mObjectSceneView->AddController(new scene::CCameraController(lCamera));
        mObjectSceneView->Resize({800,600});
        mObjectSceneView->SetTitle(L"object");
        Add(mObjectSceneView);
    }
    
/*    
    mObjectView=new scene::CSceneView;
    
    mObjectScene=new scene::CScene;
    mObjectCloud=new scene::CPointCloud;
    mObjectTransform=new scene::CMatTransform(mKinectTrans);
    mObjectScene->World()->AddChild(mObjectTransform);
    mObjectTransform->AddObject(mObjectCloud);
    mObjectView->AddScene(mObjectScene);
    mObjectCamera=new scene::CPerspectiveCamera;
    mObjectCamera->SetFOV(2*atan2(240.0,525.0));
    mObjectCamera->SetClipping(0.10,50);
    mObjectCamera->LookFrom(mKinectTrans*mat::CVec3d(0,0,0),mat::CQuat_d(mKinectTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
    mObjectView->SetCamera(mObjectCamera);
    mObjectView->AddController(new scene::CCameraController(mObjectCamera));
    mObjectView->Resize({640,480});
    mObjectView->SetTitle(L"object");
    
    Add(mObjectView);
*/
}

void CTrackerWindow::DoPlay()
{
    if(mTimer.Active())
    {
        mTimer.Disable();
    }
    else
    {
        mTimer.SetInterval(100_ms,true);
    }
}
    
void CTrackerWindow::DoStep()
{
    start_timer();
    cvi::CCloud_rgb8 lCloud=load_cloud(mURI,(mFrame++)/2);

    CTracker lTracker(mObject->GetK());
    auto lRopeFeatures=mObject->GetObjectFeatures();
    auto lFiltered=mObject->FilterCloud(lCloud);
    auto lCloudFeatures=mObject->GetCloudFeatures(lFiltered);
    end_timer("loading");
    
    lTracker.SetFeatures(std::move(lRopeFeatures),std::move(lCloudFeatures));
    lTracker.ExpectationStep(*mObject);
    lTracker.MaximizationStep(*mObject);

    mSceneCloud->SetCloud(std::move(lCloud));
    for(int i=0;i<3;i++)
    mObjectScene->StepPhysics(0.03,2,0.015);
    mCloudSceneView->Refresh();
    mObjectSceneView->Refresh();
}
    
void CTrackerWindow::OnCommitHook(const CRegion &pRegion)
{
    msg_info() << "oncommithook\n";
    CSize_i lSize((int)Size().x/4*4,(int)Size().y/4*4);
    img::CImage_rgb8 lImage(lSize);
     
    start_timer();
    glReadBuffer(GL_FRONT);
    glReadPixels(0,0,lSize.x,lSize.y,GL_RGB,GL_UNSIGNED_BYTE,lImage.RawData());
    glReadBuffer(GL_BACK);
    end_timer();
    
    img::CImage_rgb8 lFlipped(lSize);
    img::copy_pixels(img::vflip_view(lImage),lFlipped);
    
    img::save(lFlipped,io::file("commit"+to_string(mFrame,5,'0')+".ppm"));
}

/*namespace tracker*/ } /*namespace buola*/ }
